close all
clear all
tic;

%Choose options
sampleSizeMin=5;%5
sampleSizeMax=5005;%1005
eta = sqrt(1);
sigmak = 1.5;
sigmax = 1/(2*sigmak);
k0 = 0;
displacement = 1/(4*sigmak);
sampleBinsNumber = 26;%26
iterationsR = 100000;



binsS= 1000;
sMin = max([0 displacement-1.5]);
sMax = displacement+1.5;
spaceScan = sMin:(sMax-sMin)/binsS:sMax-(sMax-sMin)/binsS;

%Initialising
if sampleBinsNumber==1
    sampleSize=sampleSizeMax;
else
    sampleSize = sampleSizeMin : (sampleSizeMax-sampleSizeMin)/(sampleBinsNumber-1) : sampleSizeMax;
end


estimatedDisplacement = zeros(iterationsR,sampleBinsNumber);
parfor m = 1 : iterationsR
    %Start sampling

    sampleLoop = zeros(sampleSizeMax,3);
    numberOfCoincidences = 0;
    for i=1:sampleSizeMax
        isCoincidence = rand < (1-eta^2*exp((-(displacement*sigmak)^2)/4))/2;
        numberOfCoincidences = numberOfCoincidences + isCoincidence;
        candidates = k0 + sigmak*randn(1,2);
        while rand >= (1 + (1-2*isCoincidence) * eta^2 * cos((candidates(1)-candidates(2)) * displacement/2))/2
            candidates = k0 + sigmak*randn(1,2);
        end
        sampleLoop(i,:) = [isCoincidence candidates(1) candidates(2)];

    end

    %Evaluating Likelihood

    LLogLoop = zeros(binsS,sampleBinsNumber);

    for j=1:binsS
        currentSampleBin = 1;
        partialSum = 0;
        for i=1:sampleSizeMax

            partialSum = partialSum + log10(exp(-((sampleLoop(i,2)-k0).^2 + (sampleLoop(i,3)-k0).^2)/(2.*sigmak.^2)).*(1 + (1-2.*sampleLoop(i,1)) .* eta.^2 .* cos((sampleLoop(i,2)-sampleLoop(i,3))./2 .* spaceScan(j))));
            if i == sampleSize(currentSampleBin)
                LLogLoop(j,currentSampleBin) = partialSum;
                currentSampleBin = currentSampleBin + 1;
            end
        end
    end


    %Likelihood Maximisation

    [maxLLog, maxInd] = max(LLogLoop);

    % Estimates
    estimatedDisplacement(m,:) = spaceScan(maxInd);

end

%Statistic Estimation

estimatedDisplacementAvg = mean(estimatedDisplacement);
estimatedDisplacementVar = var(estimatedDisplacement);
